#include "mpu6050.h"
#include "i2c.h"
#include "delay.h"
#include "stdio.h"


uint8_t MPU_Write_One_Byte(uint8_t reg_addr, uint8_t data)
{
    iic_start();
    iic_send_byte((MPU_ADDR << 1) | 0);
    if (iic_wait_ack()) {
        iic_stop();
        return 1;
    }
    iic_send_byte(reg_addr);
    iic_wait_ack();
    iic_send_byte(data);
    if (iic_wait_ack()) {
        iic_stop();
        return 1;
    }
    iic_stop();
    return 0;
}

uint8_t MPU_Read_One_Byte(uint8_t reg_addr)
{
    uint8_t res = 0;
    iic_start();
    iic_send_byte((MPU_ADDR << 1) | 0);
    iic_wait_ack();
    iic_send_byte(reg_addr);
    iic_wait_ack();
    iic_start();
    iic_send_byte((MPU_ADDR << 1) | 1);
    iic_wait_ack();
    res = iic_read_byte(0);
    iic_stop();

    return res;
}

uint8_t MPU_Write_Len(uint8_t slave_addr, uint8_t reg_addr, uint8_t length, uint8_t* pbuf)
{
    uint8_t i = 0;

    iic_start();
    iic_send_byte((slave_addr << 1) | 0);
    if (iic_wait_ack()) {
        iic_stop();
        return 1;
    }
    iic_send_byte(reg_addr);
    iic_wait_ack();
    for (i = 0; i < length; i++) {
        iic_send_byte(pbuf[i]);
        if (iic_wait_ack()) {
            iic_stop();
            return 1;
        }
    }
    iic_stop();
    return 0;
}


uint8_t MPU_Read_Len(uint8_t slave_addr, uint8_t reg_addr, uint8_t length, uint8_t* pbuf)
{
    iic_start();
    iic_send_byte((slave_addr << 1) | 0);
    if (iic_wait_ack()) {
        iic_stop();
        return 1;
    }
    iic_send_byte(reg_addr);
    iic_wait_ack();
    iic_start();
    iic_send_byte((slave_addr << 1) | 1);
    iic_wait_ack();
    while (length) {
        if (length == 1) {
            *pbuf = iic_read_byte(0);
        }
        else {
            *pbuf = iic_read_byte(1);
        }
        length--;
        pbuf++;
    }
    iic_stop();
    return 0;
}

uint8_t MPU_Init(void)
{
    uint8_t res  = 0;
    // uint8_t temp = 0;

    iic_init();
    MPU_Write_One_Byte(MPU_PWR_MGMT1_REG, 0x80);
    delay_ms(100);
    MPU_Write_One_Byte(MPU_PWR_MGMT1_REG, 0x00);
    MPU_Set_Gyro_Fsr(3);
    MPU_Set_Accel_Fsr(0);
    MPU_Set_Rate(100);
    MPU_Write_One_Byte(MPU_INT_EN_REG, 0x00);
    MPU_Write_One_Byte(MPU_USER_CTRL_REG, 0x00);
    MPU_Write_One_Byte(MPU_FIFO_EN_REG, 0x00);
    MPU_Write_One_Byte(MPU_INTBP_CFG_REG, 0x80);
    res = MPU_Read_One_Byte(MPU_DEVICE_ID_REG);
    printf("addr=%x\r\n\r\n", res);
    if (res == MPU_ADDR || res == 0x70) {
        MPU_Write_One_Byte(MPU_PWR_MGMT1_REG, 0x01);
        MPU_Write_One_Byte(MPU_PWR_MGMT2_REG, 0x00);
        MPU_Set_Rate(100);
    }
    else {
        return 1;
    }
    return 0;
}


uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
    return MPU_Write_One_Byte(MPU_GYRO_CFG_REG, fsr << 3);
}

uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
    return MPU_Write_One_Byte(MPU_ACCEL_CFG_REG, fsr << 3);
}

uint8_t MPU_Set_LPF(uint16_t lpf)
{
    uint8_t data = 0;
    if (lpf >= 188)
        data = 1;
    else if (lpf >= 98)
        data = 2;
    else if (lpf >= 42)
        data = 3;
    else if (lpf >= 20)
        data = 4;
    else if (lpf >= 10)
        data = 5;
    else
        data = 6;
    return MPU_Write_One_Byte(MPU_CFG_REG, data);
}

uint8_t MPU_Set_Rate(uint16_t rate)
{
    uint8_t data;
    if (rate > 1000)
        rate = 1000;
    else if (rate < 4)
        rate = 4;

    data = 1000 / rate - 1;
    data = MPU_Write_One_Byte(MPU_SAMPLE_RATE_REG, data);
    return MPU_Set_LPF(rate / 2);
}

// uint8_t MPU_Set_Fifo(uint8_t sens)
// {}

short MPU_Get_Temperature(void)
{
    uint8_t buf[2] = {0};
    short   raw    = 0;
    float   temp   = 0;

    MPU_Read_Len(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf);
    raw  = ((uint16_t)buf[0] << 8) | buf[1];
    temp = 36.53 + ((double)raw) / 340;
    return temp * 100;
}

uint8_t MPU_Get_Gyroscope(short* gx, short* gy, short* gz)
{
    uint8_t buf[6] = {0}, res = 0;
    res = MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf);
    if (res == 0) {
        *gx = ((uint16_t)buf[0] << 8) | buf[1];
        *gy = ((uint16_t)buf[2] << 8) | buf[3];
        *gz = ((uint16_t)buf[4] << 8) | buf[5];
    }
    return res;
}
uint8_t MPU_Get_Accelrometer(short* ax, short* ay, short* az)
{
    uint8_t buf[6] = {0}, res = 0;
    res = MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf);
    if (res == 0) {
        *ax = ((uint16_t)buf[0] << 8) | buf[1];
        *ay = ((uint16_t)buf[2] << 8) | buf[3];
        *az = ((uint16_t)buf[4] << 8) | buf[5];
    }
    return res;
}
